﻿using MathNet.Numerics;
using MathNet.Numerics.IntegralTransforms;
using MathNet.Spatial.Euclidean;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace CrankShaftResultHandler
{
    public class MathHelper
    {
        /// <summary>
        /// https://www.cnblogs.com/slgkaifa/p/7265010.html
        /// </summary>
        /// <param name="xy">输入点云</param>
        /// <param name="r">半径</param>
        /// <returns>圆心</returns>
        public static ResultHandler.CurvePoint CircleFit(List<ResultHandler.CurvePoint> xy, ref double r)
        {
            var xy0 = new ResultHandler.CurvePoint();
            int N = xy.Count;
            double sum_x = 0.0f, sum_y = 0.0f;
            double sum_x2 = 0.0f, sum_y2 = 0.0f;
            double sum_x3 = 0.0f, sum_y3 = 0.0f;
            double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;

            for (int i = 0; i < N; i++)
            {
                double x = xy[i].X;
                double y = xy[i].Y;
                double x2 = x * x;
                double y2 = y * y;
                sum_x += x;
                sum_y += y;
                sum_x2 += x2;
                sum_y2 += y2;
                sum_x3 += x2 * x;
                sum_y3 += y2 * y;
                sum_xy += x * y;
                sum_x1y2 += x * y2;
                sum_x2y1 += x2 * y;
            }

            double C, D, E, G, H;
            double a, b, c;

            C = N * sum_x2 - sum_x * sum_x;
            D = N * sum_xy - sum_x * sum_y;
            E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
            G = N * sum_y2 - sum_y * sum_y;
            H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
            a = (H * D - E * G) / (C * G - D * D);
            b = (H * C - E * D) / (D * D - G * C);
            c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

            xy0.X = a / (-2);
            xy0.Y = b / (-2);
            r = Math.Sqrt(a * a + b * b - 4 * c) / 2;
            return xy0;
        }

        public static double LinearInterpolation(double x0, double y0, double x1, double y1, double xx)
        {
            double y = (y0 - y1) / (x0 - x1) * (xx - x0) + y0;
            return y;
        }

        public static List<ResultHandler.CurvePoint> NewCurve(List<ResultHandler.CurvePoint> phir, int new_curve_size)
        {
            int size = new_curve_size;
            int pointer = 0;
            List<ResultHandler.CurvePoint> curve = new List<ResultHandler.CurvePoint>(new ResultHandler.CurvePoint[size]);
            for (int i = 0; i < size; i++)
            {
                curve[i] = new ResultHandler.CurvePoint();
                // 从-pi~pi，按照2pi/size 的间距均布
                curve[i].Phi = -Math.PI + i * Math.PI / ((int)(size / 2.0));
            }
            for (int i = 0; i < size; i++)
            {
                while (true)
                {
                    // 从左到右从搜索输入的phir列表，找到均布的curve[i]在phir中的区间[ pointer-1, pointer ]
                    if (pointer == 0)
                    {
                        if (curve[i].Phi <= phir[0].Phi)
                        {
                            pointer = 0;
                            break;
                        }
                    }
                    else if (pointer == phir.Count - 1)
                    {
                        // 到最后一个点了
                        pointer = phir.Count - 1;
                        break;
                    }
                    else
                    {
                        // 找到了curve[i]所在的区间
                        if (curve[i].Phi <= phir[pointer].Phi && curve[i].Phi > phir[pointer - 1].Phi)
                            break;
                    }
                    pointer++;
                }
                if (pointer == 0)
                    curve[i].p = LinearInterpolation(phir[0].Phi, phir[0].p, phir[1].Phi, phir[1].p, curve[i].Phi);
                else
                    curve[i].p = LinearInterpolation(phir[pointer].Phi, phir[pointer].p, phir[pointer - 1].Phi, phir[pointer - 1].p, curve[i].Phi);
            }
            return curve;
        }

        public static double[] FFTFilter(double[] signal, double f1 = 1, double f2 = 20)
        {
            int N = signal.Length; // N是奇数
            double dt = 1.0 / N; // all points set in One second
            double[] n = new double[N];
            double[] t = new double[N];
            double[] filtered_signal;
            double[] f = new double[(int)(N / 2.0) + 1]; // 301
            double Fs = 1 / dt; // Sampling frequency
            for (int i = 0; i < N; i++)
            {
                n[i] = i + 1;
                t[i] = n[i] * dt;
            }
            for (int i = 0; i < f.Length; i++)
            {
                f[i] = i;
            }
            Complex32[] filtered_freq = new Complex32[N];
            Complex32[] Y = new Complex32[N];
            for (int i = 0; i < N; i++)
            {
                Y[i] = new Complex32((float)signal[i], 0);
                filtered_freq[i] = new Complex32();
            }
            Fourier.Forward(Y, FourierOptions.NoScaling);
            List<double> P2 = Y.Select(y => 2 * Complex32.Abs(y) / N).ToList();
            var P1 = P2.GetRange(0, (N - 1) / 2 + 1);
            for (int m = 0; m < N; m++)
            {
                var ff = m;
                // Filters the frequency into this frequency range and its greater than the Nyquist wave
                if (!((ff > f1 && ff < f2) || (ff > (Fs - f2) && ff < (Fs - f1))))
                    filtered_freq[m] = Y[m];
            }
            Fourier.Inverse(filtered_freq, FourierOptions.Matlab);
            filtered_signal = filtered_freq.Select(p => (double)p.Real).ToArray();
            return filtered_signal;
        }

        public static double PointToLine3D(ResultHandler.CurveLine line, ResultHandler.CurvePoint p0, ref ResultHandler.CurvePoint pd)
        {
            /** @matlab
            d = norm(cross((p0 - p1), (p0 - p2))) / norm((p2 - p1));
            t = -dot((p1 - p0), (p2 - p1)) / power(norm(p2 - p1), 2);
            x1 = p1(1);
            y1 = p1(2);
            z1 = p1(3);
            x2 = p2(1);
            y2 = p2(2);
            z2 = p2(3);
            pd = [x1 + (x2 - x1) * t, y1 + (y2 - y1) * t, z1 + (z2 - z1) * t];
            https://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
            */
            var p1 = line.start;
            var p2 = line.end;
            var vector0 = new Vector3D( p0.X, p0.Y, p0.Z );
            var vector1 = new Vector3D( p1.X, p1.Y, p1.Z );
            var vector2 = new Vector3D( p2.X, p2.Y, p2.Z );
            double d = (vector0 - vector1).CrossProduct(vector0 - vector2).ToVector().L2Norm() / (vector2 - vector1).ToVector().L2Norm();
            double t = -(vector1 - vector0).DotProduct(vector2 - vector1) / Math.Pow((vector2 - vector1).ToVector().L2Norm(), 2);
            pd.X = vector1.X + (vector2.X - vector1.X) * t;
            pd.Y = vector1.Y + (vector2.Y - vector1.Y) * t;
            pd.Z = vector1.Z + (vector2.Z - vector1.Z) * t;
            return d;
        }

        public static string DegreeToDms(double d)
        {
            int degree = Convert.ToInt16(Math.Truncate(d));  //度
            d = (d - degree) * 60.0;
            int minute = Convert.ToInt16(Math.Truncate(d));  //分
            d = (d - minute) * 60.0;
            int second = Convert.ToInt16(Math.Round(d));
            string sig = d >= 0 ? "+" : "-";
            return $"{sig}{Math.Abs(degree)}.{Math.Abs(minute):D2}.{Math.Abs(second):D2}";
        }

        // 向量夹角，返回角度(deg)
        public static double GetVectorAngle(Vector3D v1, Vector3D v2)
        {
            double cos_ang = v1.DotProduct(v2) / (v1.ToVector().L2Norm() * v2.ToVector().L2Norm());
            var ret = Math.Acos(cos_ang) / Math.PI * 180.0;
            return ret;
        }

        public static double GetRectangularAngle(double x, double y)
        {
            if (x == 0)
                return Math.PI / 2.0 * Math.Sign(y);
            if (x > 0)
                return Math.Atan(y / x);
            else
                return Math.Atan(y / x) + Math.PI;
        }

        public static double ReduceAngle(double value, double m)
        {
            var s = Math.Truncate(value / m);
            var ret = value - s * m;
            if(Math.Abs(ret)>m/2)
            {
                ret = ret - Math.Sign(ret) * m;
            }
            return ret;
        }

    }

}
